//---------------------------------------------------------------------------
//    RobotSee Interpreter - Written by Eric Gregori ( www.EMGRobotics.com )
//    Copyright (C) 2010  Eric Gregori
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.
//---------------------------------------------------------------------------
//
//*********************************************************************************************************************8
// Robot Vision Toolkit by Eric Gregori
//
// RobotSee FSLBOT and TWR-MECH Language Extension
//
// int fslbot() - returns Language Extension Version Number
// int fslbotopen( port, baud ) - Opens a connection to a SCP compatable device ( returns 0 on success )
// int servo( channel, position ) - Moves a servo to a desired position ( returns 0 on success )
// int servos( pos0, pos1, pos2, pos3 ) - Moves all 4 servos at once ( returns 0 on success )
// int iicread( address, register, data, bytes ) - Read from a i2c device ( returns 0 on success )
// int iicwrite( address, register, data ) - Write to a i2c device ( returns 0 on success )
// int adread( channel ) - Reads a AD channel
// int spisetup( baud, mode ) - Setup spi protocol
// int spitxrx( outdata, indata ) - spi xfer
// int accelread( datax, datay, datax ) - reads 3 channels of accelerometer
// int gpioinit( data ) - Sets direction of J54 GPIO pins
// int gpioout( data ) - Sets state of GPIO pins setup as output
// int gpioin( data ) - Gets state of GPIO pins as input
// int fslled( data ) - Write to LED's
//
//*********************************************************************************************************************8
#include <windows.h>
#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include "Serial_Port.h"

Serial_Port *P_SerialPortObject = 0;
int Timeout;
const char BinToASCII[] = "0123456789ABCDEF"; 

unsigned char asciitobin( unsigned char ascii )
{
	switch( ascii )
	{
		case '0': return(0);
		case '1': return(1);
		case '2': return(2);
		case '3': return(3);
		case '4': return(4);
		case '5': return(5);
		case '6': return(6);
		case '7': return(7);
		case '8': return(8);
		case '9': return(9);
		case 'a': return(10);
		case 'b': return(11);
		case 'c': return(12);
		case 'd': return(13);
		case 'e': return(14);
		case 'f': return(15);
		case 'A': return(10);
		case 'B': return(11);
		case 'C': return(12);
		case 'D': return(13);
		case 'E': return(14);
		case 'F': return(15);
		default:  return( 16 );
	}
}

int OpenSerialPort( int port, int baud, int flags, int l_timeout )
{
	unsigned char ascii_in[128];
	int index, old_index, waiting;

	P_SerialPortObject = new Serial_Port( baud, port, flags );
	Timeout = l_timeout;
	index = 0;
	for( waiting=100; waiting; --waiting )
	{
		Sleep( 5 );
		old_index = index;
		index += P_SerialPortObject->Serial_Read( (128-index), (char*)&ascii_in[index] );
		for( ; old_index<index; ++old_index )
		{
			if( ascii_in[old_index] == ';' )
			{
				index = old_index;
				old_index = -1;
				break;
			}
		}

		if( old_index == -1 ) return 0;
	}

	return 0;
}


int AutoDetect( void )
{
	int port;
	int temp, index, old_index, waiting;
	unsigned char out_buffer[128], ascii_in[128];

	for( port = 1; port<20; ++port )
	{
		if( Serial_Test( port ) )
		{
			P_SerialPortObject = new Serial_Port( 9600, port, 0x03 );
			if( P_SerialPortObject == 0 ) continue;

			out_buffer[0] = '<';
			out_buffer[1] = '0';
			out_buffer[2] = '0';
			out_buffer[3] = '0';
			out_buffer[4] = '0';
			out_buffer[5] = '>';
			temp = P_SerialPortObject->Serial_Send( (const char *)out_buffer, 6 );
			if( temp != 6 )
			{
				delete P_SerialPortObject;
				continue;
			}

			index = 0;
			for( waiting=10; waiting; --waiting )
			{
				Sleep( 5 );
				old_index = index;
				index += P_SerialPortObject->Serial_Read( (128-index), (char*)&ascii_in[index] );
				for( ; old_index<index; ++old_index )
				{
					if( ascii_in[old_index] == ';' && ascii_in[0] == '^' )
					{
						delete P_SerialPortObject;
						return( port );
					}
				}
			}

			delete P_SerialPortObject;
		} // if
	} // for port

	return( 0 );
}


// <ascii data+checksum> {ascii data+checksum}
//
// return -1;   Serial Error or timeout
// return 0;    {+} ack 
// return -2;   {-} nack
// return -3;   in buffer overrun
// return 1-128 data bytes returned with valid checksum
int SCPProtocol( unsigned char out[], int out_size, unsigned char in[], int in_size, int timeout )
{
	unsigned char out_buffer[128], ascii_in[128];
	int i, index, old_index, waiting, temp;
	unsigned char CheckSum;

	CheckSum = 0;
	out_buffer[0] = '<';
	for( i=0; i<out_size; i++ )
	{
		CheckSum += out[i];
		out_buffer[(i*2)+1] = BinToASCII[out[i]>>4];
		out_buffer[(i*2)+2] = BinToASCII[out[i]&0x0f];
	}

	CheckSum  = 0x100 - CheckSum;
	out_buffer[(i*2)+1] = BinToASCII[CheckSum>>4];
	out_buffer[(i*2)+2] = BinToASCII[CheckSum&0x0f];
	out_buffer[(i*2)+3] = '>';

	P_SerialPortObject->Serial_Reset();
	temp = P_SerialPortObject->Serial_Send( (const char *)out_buffer, (i*2)+4 );
	if( temp != (i*2)+4 ) return -1;
 
	index = 0;
	for( waiting=(timeout/5); waiting; --waiting )
	{
		Sleep( 5 );
		old_index = index;
		index += P_SerialPortObject->Serial_Read( (128-index), (char*)&ascii_in[index] );
		for( ; old_index<index; ++old_index )
		{
			if( ascii_in[old_index] == '}' )
			{
				index = old_index;
				old_index = -1;
				break;
			}
		}

		if( old_index == -1 ) break;
	}

	if( waiting == 0 ) return -4;

	if( index == 2 )
	{
		// {+} or {-}
		if( ascii_in[0] != '{' ) return -5;
		if( ascii_in[2] != '}' ) return -6;
		if( ascii_in[1] == '+' ) return 0; 
		if( ascii_in[1] != '-' ) return -7; 
		return -2;
	}

	// Translate ASCII to bin
	CheckSum = 0;
	for( i=1; i<index; i+=2 )
	{
		if( ((i-1)/2) >= in_size ) return -3;
		in[(i-1)/2] = asciitobin(ascii_in[i])<<4;
		in[(i-1)/2] += asciitobin(ascii_in[i+1])&0x0f;
		CheckSum += in[(i-1)/2];
	}

	if( CheckSum != 0 ) return -8;

	return( ((i-1)/2)+1 );
}


int servo( int id, unsigned char pos )
{
	unsigned char out[64], in[64];
	int retval;

	if( P_SerialPortObject == 0 ) return -1;

	if( id < 0 || id > 3 ) return -1;

	out[0] = id+1;
	out[1] = pos;

	retval = SCPProtocol( out, 2, in, 128, Timeout );
	if( retval < 0 ) 
		return( retval );

	return(0);	
}


int servos( unsigned char pos0, unsigned char pos1, unsigned char pos2, unsigned char pos3 )
{
	unsigned char out[64], in[64];
	int retval;

	if( P_SerialPortObject == 0 ) return -1;

	out[0] = 0x09;
	out[1] = pos0;
	out[2] = pos1;
	out[3] = pos2;
	out[4] = pos3;

	retval = SCPProtocol( out, 5, in, 128, Timeout );
	if( retval < 0 ) return( retval );
	return(0);	
}

int i2cwrite( unsigned char device, unsigned char reg, unsigned char data )
{
	unsigned char out[64], in[64];
	int retval;

	if( P_SerialPortObject == 0 ) return -1;

	out[0] = 11;
	out[1] = device;
	out[2] = reg;
	out[3] = data;

	retval = SCPProtocol( out, 4, in, 128, Timeout );
	if( retval < 0 ) return( retval );
	return(0);	
}

int i2cread( unsigned char device, unsigned char reg, unsigned int *data, unsigned int bytes )
{
	unsigned char out[64], in[64];
	int retval;
	unsigned long ldata;

	if( P_SerialPortObject == 0 ) return -1;

	if( bytes > 4 ) bytes = 4;

	out[0] = 12;
	out[1] = device;
	out[2] = reg;
	out[3] = bytes;

	retval = SCPProtocol( out, 4, in, 128, Timeout );
	if( retval < 0 ) return( retval );

	ldata = 0;
	switch( bytes )
	{
		case 1:
		default:
			ldata = in[0];
			break;

		case 2:
			ldata = in[0];
			ldata <<= 8;
			ldata += in[1];
			break;

		case 3:
			ldata = in[0];
			ldata <<= 8;
			ldata += in[1];
			ldata <<= 8;
			ldata += in[2];
			break;

		case 4:
			ldata = in[0];
			ldata <<= 8;
			ldata += in[1];
			ldata <<= 8;
			ldata += in[2];
			ldata <<= 8;
			ldata += in[3];
			break;
	}
	*data = ldata;

	return(0);	
}

int led( int data )
{
	unsigned char out[64], in[64];
	int retval;

	if( P_SerialPortObject == 0 ) return -1;

	out[0] = 13;	// Write LED SCP command
	out[1] = data;  // Data to LED's

	retval = SCPProtocol( out, 2, in, 128, Timeout );
	if( retval < 0 ) 
		return( retval );

	return(0);	
}

int accelread( int *ax, int *ay, int *az )
{
	unsigned char out[64], in[64];
	int retval;

	if( P_SerialPortObject == 0 ) return -1;

	out[0] = 10; // Read Accel SCP command

	retval = SCPProtocol( out, 1, in, 128, Timeout );
	if( retval < 0 ) 
		return( retval );

	// 0 = X lsb
	// 1 = X msb
	// 2 = Y lsb
	// 3 = Y msb
	// 4 = Z lsb
	// 5 = z msb

	*ax = in[0]+(in[1]*0x10);
	*ay = in[2]+(in[3]*0x10);
	*az = in[4]+(in[5]*0x10);
	return(0);
}

int ADread( int *ad )
{
	unsigned char out[64], in[64];
	int retval;

	if( P_SerialPortObject == 0 ) return -1;

	out[0] = 17; // Read AD SCP command 
	out[1] = 0;	 // AD channel

	retval = SCPProtocol( out, 2, in, 128, Timeout );
	if( retval < 0 ) 
		return( retval );

	*ad = in[0];
	return(0);
}

int TOUCHread( int *ad )
{
	unsigned char out[64], in[64];
	int retval;

	if( P_SerialPortObject == 0 ) return -1;

	out[0] = 18; // Read Touch SCP command 

	retval = SCPProtocol( out, 1, in, 128, Timeout );
	if( retval < 0 ) 
		return( retval );

	*ad = in[0]+(in[1]*0x10);
	return(0);
}

int gpioinit( int direction )	// 0 = output
{
	unsigned char out[64], in[64];
	int retval;

	if( P_SerialPortObject == 0 ) return -1;

	out[0] = 14; // GPIOINIT SCP command 
	out[1] = direction;	 // 

	retval = SCPProtocol( out, 2, in, 128, Timeout );
	if( retval < 0 ) 
		return( retval );

	return(0);
}

int gpioout( int data )
{
	unsigned char out[64], in[64];
	int retval;

	if( P_SerialPortObject == 0 ) return -1;

	out[0] = 15; // GPIOOUT SCP command 
	out[1] = data;	 // 

	retval = SCPProtocol( out, 2, in, 128, Timeout );
	if( retval < 0 ) 
		return( retval );

	return(0);
}

int gpioin( int *ad )
{
	unsigned char out[64], in[64];
	int retval;

	if( P_SerialPortObject == 0 ) return -1;

	out[0] = 16; // GPIOIN SCP command 

	retval = SCPProtocol( out, 1, in, 128, Timeout );
	if( retval < 0 ) 
		return( retval );

	*ad = in[0];
	return(0);
}

#define EOF (-1)


void menu( void )
{
	int i,j;

	while( 1 )
	{
		printf( "\nTWR-MECH / FSLBOT Testing utility by Eric Gregori ( www.EMGRobotics.com )\n" );
		printf("\n0) Exit" );
		printf("\n1) Wiggle J31 servo" );
		printf("\n2) Wiggle J32 servo" );
		printf("\n3) Wiggle J33 servo" );
		printf("\n4) Wiggle J34 servo" );
		printf("\n5) Wiggle J31-J35 servos" );
		printf("\n6) LED test" );
		printf("\n7) AccelRead" );
		printf("\n8) AD Read" );
		printf("\n9) Touch Read" );
		printf("\nA) Read MPR121 using iicread" );
		printf("\nB) Test GPIO commands" );
		printf("\nC) test iicwrite command" );
		printf( "\n\nEnter Command> \n" ); 

		switch( _getch() )
		{
			default: break;
			case '0': exit(0);
			case '1':
				printf( "\nTesting SCP command 0 - Look for servo on J31 to wiggle around it's center" );
				for( i= 100; i< 150; i++ )
					servo( 0, i );
				break;
			case '2':
				printf( "\nTesting SCP command 1 - Look for servo on J32 to wiggle around it's center" );
				for( i= 100; i< 150; i++ )
					servo( 1, i );
				break;
			case '3':
				printf( "\nTesting SCP command 2 - Look for servo on J33 to wiggle around it's center" );
				for( i= 100; i< 150; i++ )
					servo( 2, i );
				break;
			case '4':
				printf( "\nTesting SCP command 3 - Look for servo on J34 to wiggle around it's center" );
				for( i= 100; i< 150; i++ )
					servo( 3, i );
				break;
			case '5':
				printf( "\nTesting SCP command 9 - Look for servos on J31-J34 to wiggle around their center" );
				for( i= 100; i< 150; i++ )
					servos( i,i,i,i );
				for( i= 100; i< 150; i++ )
					servos( i,128,128,128 );
				for( i= 100; i< 150; i++ )
					servos( 128,i,128,128 );
				for( i= 100; i< 150; i++ )
					servos( 128,128,i,128 );
				for( i= 100; i< 150; i++ )
					servos( 128,128,128,i );
				break;
			case '6':
				printf( "\nTesting SCP command 13 - LED control" );
				for( i=1; i<0x101; i=i<<1 )
				{
					printf( "\nLED = %x", i ); 
					led(i);
					Sleep(500);
				}
				break;
			case '7':
				printf( "\nTesting SCP command 10 - accelread" );
				for( i=0; i<100; ++i )
				{
					int ax,ay,az;
					accelread( &ax, &ay, &az );
					printf( "\nax = %d, ay = %d, az = %d", ax, ay, az );
					Sleep( 100 );
				}
				break;
			case '8':
				printf( "\nTesting SCP command 17 - ADread" );
				for( i=0; i<100; ++i )
				{
					int ad;
					ADread( &ad );
					printf( "\nad = %d", ad );
					Sleep( 100 );
				}
				break;
			case '9':
				printf( "\nTesting SCP command 18 - Touchread" );
				for( i=0; i<100; ++i )
				{
					int ad;
					TOUCHread( &ad );
					printf( "\nTouch = %d", ad );
					Sleep( 100 );
				}
				break;
			case 'a':
			case 'A':
				printf( "\nTesting SCP command 12 - iicread" );
				printf( "\nDumping registers from the MPR121" );
				for( i=0x2B; i<0x5F; ++i )
				{
					unsigned int ad;
					i2cread( 0x5A, i, &ad, 1 );
					printf( "\nRegister %x = %x", i, ad );
					Sleep( 100 );
				}
				break;
			case 'b':
			case 'B':
				printf( "\nTesting GPIO commands - SCP 14, 15, 16" );
				gpioinit( 0x00 ); // Set to output
				gpioout( 0x55 );  // Write a pattern
				{
					int ad;
					gpioin( &ad );
					if( ad != 0x55 )
						printf( "\nFAILED!" );
				}
				break;
			case 'c':
			case 'C':
				printf( "\nTesting iicwrite command - SCP 11" );
				i2cwrite( 0x38, 0x03, 0x00 );
				for( i=1; i<0x101; i=i<<1 )
				{
					printf( "\nLED = %x", i ); 
					i2cwrite( 0x38, 0x01, i );
					Sleep(500);
				}
				break;

		}

		printf( "\nDONE!\n" );
	}
}


int main( int argc, unsigned char **argv )
{
	int	port;
	int i,j;

	printf( "\nTWR-MECH / FSLBOT Testing utility by Eric Gregori ( www.EMGRobotics.com )\n" );
	port = AutoDetect();
	if( port >= 0 )
	{
		printf( "\nAutoDetect() detected a TWR-MECH board on port: %d", port );
	}
	else
	{
		printf( "\nAutoDetect() failed to find a TWR-MECH board" );
		exit( port );
	}


	if( OpenSerialPort( port, 115200, 0x03, 50 ) < 0 )
	{
		printf( "\nERROR! - Opening SerialPort: %d", port );
		exit( 1 );
	}

	menu();

	printf( "\ntest" );
	return 0;
}

#if 0
__declspec(dllexport) int (__cdecl ExtFunctions)( char		*function, 
												  char		*parameters, 
												  int		*P_retval,
												  void		*lint_parms_array,
												  void		*lint_parms_type_array,
												  void		*lchar_parms_strings_array,
												  int		num_parms,
												  char		*error_string)
{
	// int_parms_array - Data back and forth with interpreter
	int		*int_parms_array			= (int *)lint_parms_array;

	// int_parms_type_array - data type coming in
	int		*int_parms_type_array		= (int *)lint_parms_type_array;

	// int_parms_cmd_array - command back to interpreter for this parameter
	int		*int_parms_cmd_array		= (int *)lint_parms_type_array;

	int		*retval						= (int *)*P_retval;

	// char_parms_string_array - parsed parameter string
	char	**char_parms_strings_array  = (char **)lchar_parms_strings_array;
	char	*parm1_string               = char_parms_strings_array[0];
	char	*parm2_string               = char_parms_strings_array[1];

	// int_parms_type_array - 0 = not variable, 1 = variable, set to 2 for write back
	// int_parms_array - integers values for parms if they are defined variables
	// char_parms_strings_array - array of pointers to comma delimited parms in paramters string

	error_string[0] = 0;

	// return DLL version
	// int fslbot() - returns Language Extension Version Number
	if( strcmp( function, "fslbot" ) == 0 )
	{
		*retval = REVISION;
		return(0);
	}

	// int fslbotopen( port, baud ) - Opens a connection to a SCP compatable device ( returns 0 on success )
	if( strcmp( function, "fslbotopen" ) == 0 )
	{
		if( num_parms != 4 )
		{
			sprintf( error_string, "Syntax Error - usage: scpopen( port_number, baud, flags, timeout )" );
			*retval = -1;
			return(0);
		}

		if( int_parms_array[0] == 0 )
		{
			int_parms_array[0] = AutoDetect();
			if( int_parms_array[0] <= 0 ) 
			{
				*retval = -1;
				return 0;
			}
		}

		*retval = OpenSerialPort( int_parms_array[0], int_parms_array[1], int_parms_array[2], int_parms_array[3] );
		if( P_SerialPortObject == 0 ) *retval = -1;
		return( 0 );
	}

	// servos - control all 8 servos at once
	// int servos( pos0, pos1, pos2, pos3 ) - Moves all 4 servos at once ( returns 0 on success )
	if( strcmp( function, "servos" ) == 0 )
	{
		if( P_SerialPortObject == 0 )
		{
			sprintf( error_string, "Call scpopen( port_number, baud ) first" );
			*retval = 1;
			return( 0 );
		}

		if( num_parms == 4 )
		{
			*retval = servos(	(unsigned char)int_parms_array[0], 
								(unsigned char)int_parms_array[1], 
								(unsigned char)int_parms_array[2], 
								(unsigned char)int_parms_array[3] );

		}
		else
			sprintf( error_string, "Syntax Error - usage: servos( servo0, servo1, servo2, servo3 )" );

		return( 0 );
	}

	// servo - control one servo
	// int servo( channel, position ) - Moves a servo to a desired position ( returns 0 on success )
	if( strcmp( function, "servo" ) == 0 )
	{
		if( P_SerialPortObject == 0 )
		{
			sprintf( error_string, "Call scpopen( port_number, baud ) first" );
			*retval = 1;
			return( 0 );
		}

		if( num_parms == 2 )
		{
			if( (unsigned char)int_parms_array[0] < 4 )
				*retval = servo( (unsigned char)int_parms_array[0], (unsigned char)int_parms_array[1] );
			else
				sprintf( error_string, "Syntax Error - channel > 3" );
		}
		else
			sprintf( error_string, "Syntax Error - usage: servo( servo, position )" );

		*retval = 0;
		return(0);
	}

	// int iicwrite( address, register, data ) - Write to a i2c device ( returns 0 on success )
	if( strcmp( function, "iicwrite" ) == 0 )
	{
		if( P_SerialPortObject == 0 )
		{
			sprintf( error_string, "Call scpopen( port_number, baud ) first" );
			*retval = 1;
			return( 0 );
		}

		if( num_parms == 3 )
		{
			*retval = i2cwrite( (unsigned char)int_parms_array[0], (unsigned char)int_parms_array[1], (unsigned char)int_parms_array[2] );
		}
		else
			sprintf( error_string, "Syntax Error - usage: iicwrite( address, register, data ) - Write to a i2c device ( returns 0 on success )" );

		return(0);
	}

	// int iicread( address, register, data, bytes ) - Read from a i2c device ( returns 0 on success )
	if( strcmp( function, "iicread" ) == 0 )
	{
		unsigned int data;
		if( P_SerialPortObject == 0 )
		{
			sprintf( error_string, "Call scpopen( port_number, baud ) first" );
			*retval = 1;
			return( 0 );
		}

		if( num_parms == 4 )
		{
			// int i2cread( unsigned char device, unsigned char reg, unsigned int *data, unsigned int bytes )
			*retval = i2cread( (unsigned char)int_parms_array[0], (unsigned char)int_parms_array[1], &data, (unsigned char)int_parms_array[3] );
			int_parms_array[2] = data;
			int_parms_cmd_array[2] = 2; // Write back
		}
		else
			sprintf( error_string, "Syntax Error - usage: // int iicread( address, register, data, bytes ) - Read from a i2c device ( returns 0 on success )" );

		return(0);
	}

	// int accelread( datax, datay, datax ) - reads 3 channels of accelerometer
	if( strcmp( function, "accelread" ) == 0 )
	{
		unsigned int data;
		if( P_SerialPortObject == 0 )
		{
			sprintf( error_string, "Call fslbotopen( port_number, baud ) first" );
			*retval = 1;
			return( 0 );
		}

		if( num_parms == 3 )
		{
			// int i2cread( unsigned char device, unsigned char reg, unsigned int *data, unsigned int bytes )
			*retval = i2cread( (unsigned char)int_parms_array[0], (unsigned char)int_parms_array[1], &data, (unsigned char)int_parms_array[3] );
			int_parms_array[0] = data;
			int_parms_array[1] = data;
			int_parms_array[2] = data;
			int_parms_cmd_array[0] = 2; // Write back
			int_parms_cmd_array[1] = 2; // Write back
			int_parms_cmd_array[2] = 2; // Write back
		}
		else
			sprintf( error_string, "Syntax Error - usage: int accelread( datax, datay, datax ) - reads 3 channels of accelerometer ( returns 0 on success )" );

		return(0);
	}


// int adread( channel ) - Reads a AD channel
// int spisetup( baud, mode ) - Setup spi protocol
// int spitxrx( outdata, indata ) - spi xfer
// int gpioinit( data ) - Sets direction of J54 GPIO pins
// int gpioout( data ) - Sets state of GPIO pins setup as output
// int gpioin( data ) - Gets state of GPIO pins as input

// int iicsetup( baud ) - Setup iic protocol

	return(-1);
}
#endif


